#include "d435Camera.h"

D435Camera::~D435Camera()
{
    // 停止管道
    std::cout << "Stopping D435Camera pipe..." << std::endl;
    pipe.stop();
    std::cout << "D435Camera stopped." << std::endl;
}

bool D435Camera::init()
{
    try
    {
        // 配置彩色和深度流
        pipe_config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);
        pipe_config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30); 

        // 启动管道
        rs2::pipeline_profile profile = pipe.start(pipe_config);

        // 获取深度传感器的深度标尺
        auto depth_sensor = profile.get_device().first<rs2::depth_sensor>();
        if (depth_sensor)
        {
            depth_scale = depth_sensor.get_depth_scale();
            std::cout << "Depth scale: " << depth_scale << std::endl;
        }

        // 初始化对齐对象（对齐到彩色）
        align_to_color = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
        return true;
    }
    catch (const rs2::error &e)
    {
        std::cerr << "RealSense error: " << e.what() << std::endl;
        return false;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        return false;
    }
}

bool D435Camera::getFrame(cv::Mat &color, cv::Mat &depth)
{
    try
    {
        // 等待一组帧
        rs2::frameset frames = pipe.wait_for_frames();

        // 对齐深度帧到彩色帧
        rs2::frameset aligned_frames = align_to_color->process(frames);

        // 获取对齐后的彩色和深度帧
        rs2::video_frame color_frame = aligned_frames.get_color_frame();
        rs2::depth_frame depth_frame = aligned_frames.get_depth_frame();

        if (!color_frame || !depth_frame)
        {
            std::cerr << "Warning: empty color or depth frame!" << std::endl;
            return false;
        }

        // 转换为OpenCV格式
        color = cv::Mat(cv::Size(640, 480), CV_8UC3, (void *)color_frame.get_data(), cv::Mat::AUTO_STEP);
        depth = cv::Mat(cv::Size(640, 480), CV_16UC1, (void *)depth_frame.get_data(), cv::Mat::AUTO_STEP);

        return true;
    }
    catch (const rs2::error &e)
    {
        std::cerr << "RealSense error: " << e.what() << std::endl;
        return false;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        return false;
    }
}

bool D435Camera::getFrame(cv::Mat &color)
{
    try
    {
        // 等待一组帧
        rs2::frameset frames = pipe.wait_for_frames();

        // 获取彩色帧
        rs2::video_frame color_frame = frames.get_color_frame();

        if (!color_frame)
        {
            std::cerr << "Warning: empty color frame!" << std::endl;
            return false;
        }

        // 转换为OpenCV格式
        color = cv::Mat(cv::Size(640, 480), CV_8UC3, (void *)color_frame.get_data(), cv::Mat::AUTO_STEP);

        return true;
    }
    catch (const rs2::error &e)
    {
        std::cerr << "RealSense error: " << e.what() << std::endl;
        return false;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        return false;
    }
}
